Import yimWork
Import box2d.collision
Import box2d.collision.shapes
Import box2d.dynamics.joints
Import box2d.common.math
Import box2d.dynamics.contacts
Import box2d.dynamics

Class yimBox2DWorld
	Field world:b2World
	Field gravity:b2Vec2
	Field doSleep:Bool
	
	Field timeStep:Float = 1.0/60.0
	Field physScale:Float = 30
    Field velocityIterations:int = 10
    Field positionIterations:int = 10
	
	Field eng:yimEngine
	Field dbgDraw:b2DebugDraw
	
	Field ground:b2Body
	
	Method New(engine:yimEngine, gravX:Float, gravY:Float)
		eng = engine

        '// Define the gravity vector
        gravity = New b2Vec2(gravX, gravY)
        '// Allow bodies to sleep
        doSleep = True
        '// Construct a world object
        world = New b2World(gravity, doSleep)
        world.SetWarmStarting(True)
        '// set debug draw
        dbgDraw = New b2DebugDraw()
        
        'dbgDraw.SetSprite(m_sprite)
        dbgDraw.SetDrawScale(30.0, -30.0)
        dbgDraw.SetFillAlpha(0.3)
        dbgDraw.SetLineThickness(1.0)
        dbgDraw.SetFlags(b2DebugDraw.e_shapeBit | b2DebugDraw.e_jointBit)'| b2DebugDraw.e_pairBit)
        
		ground = world.GetGroundBody()
		
	End
	
	Method Update()
		world.TimeStep(timeStep, velocityIterations, positionIterations)
        world.ClearForces()
	End
	
	Method Render()
		world.DrawDebugData(False)
	End
	
	Method Reset()
		DestroyBodies()
		DestroyJoints()
		world.SetGravity(gravity)
	End
	
	Method SetDebugDraw(torf:Bool)
		If torf
			world.SetDebugDraw(dbgDraw)	
		Else
			world.SetDebugDraw(Null)
		EndIf
	End
	
	Method SetPhysRate(rate:Float)
		timeStep = 1.0/rate
	End
	
	Method SetPhysScale(sca:Float)
		physScale = sca
	End
	
	Method SetGravity(x:Float, y:Float)
		world.SetGravity(New b2Vec2(x, y))
	End
	
	Method DestroyBodies()
		Local curBody:b2Body = world.GetBodyList()
		
		While curBody
			If curBody <> ground
				world.DestroyBody(curBody)
			EndIf
			curBody = curBody.GetNext()
		Wend
	End
	
	Method DestroyJoints()
		Local curJoint:b2Joint = world.GetJointList()
		While curJoint
			world.DestroyJoint(curJoint)
			curJoint = curJoint.GetNext()
		Wend
	End
	
	Method CreateEdgeFixture:b2FixtureDef(x1:Float, y1:Float, x2:Float, y2:Float, dens:Float, fric:Float, rest:Float)
		Local sv:b2Vec2 = New b2Vec2(x1/physScale,y1/physScale)
		Local ev:b2Vec2 = New b2Vec2(x2/physScale,y2/physScale)
		
		Local sd :b2PolygonShape = New b2PolygonShape()
		sd.SetAsEdge(sv,ev)
		Local fixtureDef :b2FixtureDef = New b2FixtureDef()
		
		fixtureDef.shape = sd
		fixtureDef.density = dens
		fixtureDef.friction = fric
		fixtureDef.restitution = rest
		
		
		Return fixtureDef
	End
	
	Method CreateEdge:b2Body(x1:Float, y1:Float, x2:Float, y2:Float, density:Float, friction:Float, restitution:Float, isStatic:Bool)
		Local body :b2Body
        Local bd :b2BodyDef = New b2BodyDef()
        
		If isStatic
			bd.type = b2Body.b2_staticBody
		Else
			bd.type = b2Body.b2_Body
		EndIf
		
        body = world.CreateBody(bd)
        body.CreateFixture(CreateEdgeFixture(x1, y1, x2, y2, density, friction, restitution))
		
		Return body
	End
	
	Method CreateCircleFixture:b2FixtureDef(radius:Float, dens:Float, fric:Float, rest:Float, localX:Float=0.0, localY:Float=0.0)
		Local circShape :b2CircleShape = New b2CircleShape(radius / physScale)
		Local fixtureDef :b2FixtureDef = New b2FixtureDef()
		
		fixtureDef.shape = circShape
		fixtureDef.density = dens
		fixtureDef.friction = fric
		fixtureDef.restitution = rest
		
		circShape.SetLocalPosition(New b2Vec2(localX, localY))

		Return fixtureDef
	End
	
	Method CreateCircle:b2Body(x:Float, y:Float, radius:Float, density:Float, friction:Float, restitution:Float, isStatic:Bool)
		Local body :b2Body
		Local bodyDefC :b2BodyDef = New b2BodyDef()
		
		If isStatic
			bodyDefC.type = b2Body.b2_staticBody
		Else
			bodyDefC.type = b2Body.b2_Body
		EndIf
		
		
		bodyDefC.position.Set(x / physScale, y / physScale)
		bodyDefC.angle = 0
		body = world.CreateBody(bodyDefC)
		body.CreateFixture(CreateCircleFixture(radius, density, friction, restitution))
		
		Return body
	End
	
	Method CreateBoxFixture:b2FixtureDef(w:Float, h:Float, dens:Float, fric:Float, rest:Float, localX:Float=0.0, localY:Float=0.0)
		Local boxShape :b2PolygonShape = New b2PolygonShape()
		Local fixtureDef :b2FixtureDef = New b2FixtureDef()
		
		fixtureDef.shape = boxShape
		fixtureDef.density = dens
		fixtureDef.friction = fric
		fixtureDef.restitution = rest
		
		boxShape.SetAsOrientedBox((w/2) / physScale, (h/2) / physScale, New b2Vec2(localX, localY))
		
		Return fixtureDef
	End
	
	Method CreateBox:b2Body(x:Float, y:Float, w:Float, h:Float, density:Float, friction:Float, restitution:Float, isStatic:Bool)
		Local body :b2Body
		Local bodyDef :b2BodyDef = New b2BodyDef()

		
		If isStatic
			bodyDef.type = b2Body.b2_staticBody
		Else
			bodyDef.type = b2Body.b2_Body
		EndIf
		

		bodyDef.position.Set(x / physScale, y / physScale)
		bodyDef.angle = 0
		
		body = world.CreateBody(bodyDef)
		body.CreateFixture(CreateBoxFixture(w, h, density, friction, restitution))
		
		Return body
	End
	
	Method CreateBody:b2Body(x:Float, y:Float, isStatic:Bool)
		Local bd:b2BodyDef = New b2BodyDef()
        
		If isStatic
			bd.type = b2Body.b2_staticBody
		Else
			bd.type = b2Body.b2_Body
		EndIf
            
        bd.position.Set(x/physScale,y/physScale)
        bd.angle = 0
		
        Return world.CreateBody(bd)
	End
	
	Method CreateRevoluteJoint:b2Joint(body1:b2Body, body2:b2Body)
		If Not body2
			body2 = ground
		EndIf
        Local jd :b2RevoluteJointDef = New b2RevoluteJointDef()
        jd.Initialize(body2, body1, body1.GetPosition())
        
		Return world.CreateJoint(jd)
	End
	
	Method DeleteJoint(joint:b2Joint)
		If JointExists(joint)
			world.DestroyJoint(joint)
		EndIf
	End
	
	Method DeleteBody(body:b2Body)
		If BodyExists(body)
			world.DestroyBody(body)
		EndIf
	End
	
	Method JointExists:Bool(joint:b2Joint)
		Local curJoint:b2Joint = world.GetJointList()
		While curJoint
			If curJoint = joint
				Return True
			EndIf
			curJoint = curJoint.GetNext()
		Wend
		Return False
	End
	
	Method BodyExists:Bool(body:b2Body)
		Local curBody:b2Body = world.GetBodyList()
		
		While curBody
			If curBody = body
				Return True
			EndIf
			curBody = curBody.GetNext()
		Wend
		Return False
	End
	
	Method GetBodyCount:Int()
		Return world.GetBodyCount()
	End
	
	Method GetJointCount:Int()
		Return world.GetJointCount()
	End
End

Class yimBox2DBody
	Field body:b2Body
	
	Method GetPosition:b2Vec2()
		Return body.GetPosition()
	End
	
	Method GetAngle:Float()
		Return body.GetAngle()
	End
End

Class yimBox2DJoint
	Field joint:b2Joint
	
End